#ifndef __ALG_PID_H__
#define __ALG_PID_H__

/*=============================Include========================================*/
#include "stdint.h"
/*==============================ExportedMacros================================*/

/*=============================Exported_types==================================*/
typedef enum d_first : uint8_t {
	D_First_DISABLE = 0,
	D_First_ENABLE,
} d_first_e;

class PID {
   protected:
	// PID计时器周期, s
	float d_t;
	// 死区, Error在其绝对值内不输出
	float dead_zone;
	// 微分先行
	d_first_e d_first;

	float i_out_max;  // 积分输出最大值
	float out_max;

	// pid
	float kp = 0.0f;
	float ki = 0.0f;
	float kd = 0.0f;
	float kf = 0.0f;

	float out = 0.0f;  // PID总输出值

	// 变速积分定速内段阈值, 0为不限制
	float i_variable_speed_A = 0.0f;
	// 变速积分变速区间, 0为不限制
	float i_variable_speed_B = 0.0f;
	// 积分分离阈值，需为正数, 0为不限制
	float i_separate_threshold = 0.0f;

	// 设定值与反馈值
	float target = 0.0f;  // 当前目标设定值
	float ref = 0.0f;     // 当前反馈值 (实际值)

	float integral_error = 0.0f;  // 积分值

	// 历史数据存储
	float last_out = 0.0f;
	float last_target = 0.0f;  // 上一次目标设定值
	float last_error = 0.0f;   // 前次误差
	float last_ref = 0.0f;     // 上一次反馈值

   public:
	void init(float _kp, float _ki, float _kd, float _kf = 0.0f,
	          float _i_out_max = 0.0f, float _out_max = 0.0f,
	          float _d_t = 0.001f, float _dead_zone = 0.0f,
	          float _i_variable_speed_A = 0.0f,
	          float _i_variable_speed_B = 0.0f,
	          float _i_separate_threshold = 0.0f,
	          d_first_e _d_first = D_First_DISABLE);

	inline float get_out();
	inline float get_ref();
	inline float get_target();
	inline float get_integral_error();
	inline float get_kp();
	inline float get_ki();
	inline float get_kd();
	inline float get_kf();

	inline void set_kp(float _kp);
	inline void set_ki(float _ki);
	inline void set_kd(float _kd);
	inline void set_kf(float _kf);
	inline void set_i_out_max(float _i_out_max);
	inline void set_out_max(float _out_max);
	inline void set_i_variable_speed_A(float _i_variable_speed_A);
	inline void set_i_variable_speed_B(float _i_variable_speed_B);
	inline void set_i_separate_threshold(float _i_separate_threshold);
	inline void set_target(float _target);
	inline void set_ref(float _ref);
	inline void set_integral_error(float _integral_error);

	void calc();
};
/*=============================ExportedDefinitions============================*/

/*=============================ExportedFunctionDeclarations===================*/
float PID::get_out() { return out; }
float PID::get_ref() { return ref; }
float PID::get_target() { return target; }
float PID::get_integral_error() { return (integral_error); }
float PID::get_kp() { return kp; }
float PID::get_ki() { return ki; }
float PID::get_kd() { return kd; }
float PID::get_kf() { return kf; }

void PID::set_kp(float _kp) { kp = _kp; }
void PID::set_ki(float _ki) { ki = _ki; }
void PID::set_kd(float _kd) { kd = _kd; }
void PID::set_kf(float _kf) { kf = _kf; }
void PID::set_i_out_max(float _i_out_max) { i_out_max = _i_out_max; }
void PID::set_out_max(float _out_max) { out_max = _out_max; }
void PID::set_i_variable_speed_A(float _i_variable_speed_A)
{
	i_variable_speed_A = _i_variable_speed_A;
}
void PID::set_i_variable_speed_B(float _i_variable_speed_B)
{
	i_variable_speed_B = _i_variable_speed_B;
}
void PID::set_i_separate_threshold(float _i_separate_threshold)
{
	i_separate_threshold = _i_separate_threshold;
}
void PID::set_target(float _target) { target = _target; }
void PID::set_ref(float _ref) { ref = _ref; }
void PID::set_integral_error(float _integral_error)
{
	integral_error = _integral_error;
}

#endif

/*========================COPYRIGHT(C) CCUT-DAMOCLES==========================*/
